/**
*  \file can_li_ion_bms.c
*
*  \brief li_ion_bms function
*
*  \author dajin.li  <dajin.li@linde-china.cn>
*
*/

/*===============================================[ private includes  ]================================================*/
#include "can_li_ion_bms.h"
#include "cancom.h"
#include "../assertor/assertor.h"
#include "../pal/pal_mem.h"
#include "../endian/endian_read.h"
#include "../nth/nth.h"
#include "../saturated/sinc.h"
#include "../sched/sched.h"
#include "../can/can.h"
#include "../endian/endian.h"
#include "../kwpservices/kwpservices.h"
#include "../app/app.h"


/*===============================================[ private defines   ]================================================*/

/*MotorL PDO1 0x18E*/

/*MotorL PDO2 0x28E*/

/*MotorL PDO3 0x38E*/

/*MotorT PDO1 0x18A*/

/*MotorT PDO2 0x28A*/

/*MotorT PDO3 0x38A*/

/*DebugV PDO0 0x625*/

/*Motor Heartbeat 0x70A*/

/*Timestamp 0xff*/




/*===============================================[ private datatypes ]================================================*/
struct can_li_ion_rx_signal
{
    enum command_type command;
    enum node_states bms_state;
    union
    {
        U8  u8data[8];
        struct
        {
            S32 bms_battery_current:24;               /*Byte0-2 0  :23*/
            S32 bms_battery_volts:24;                 /*Byte3-5 24 :47*/
            U8  bms_battery_state;                    /*Byte6   48 :55*/
            U8  bms_soh;                              /*Byte7   56 :63*/
        }store;
    }bms_rx_pdo1;

    union
    {
        U8  u8data[8];
        struct
        {
            U16 bms_max_charge_current2;        /*Byte0:1 0  :15*/
            U16 bms_max_discharge_current2;     /*Byte2:3 16 :31*/
            U16 bms_max_charge_current100;      /*Byte4:5 32 :47*/
            U16 bms_max_discharge_current100;   /*Byte6:7 48 :63*/
        }store;
    }bms_rx_pdo2;

    union
    {
        U8  u8data[8];
        struct
        {
            U16 bms_dtc1;              /*Byte0:1 0  :15*/
            U16 bms_dtc2;              /*Byte2:3 16 :31*/
            U16 bms_dtc3;              /*Byte4:5 32 :47*/
            U16 bms_dtc4;              /*Byte6:7 48 :63*/
        }store;
    }bms_rx_pdo3;

    union
    {
        U8  u8data[8];
        struct
        {
            U32 bms_min_discharge_volts:24;     /*Byte0:2 0  :23*/
            U32 bms_max_charge_volts:24;        /*Byte3:5 24 :47*/
            U8  bms_soc_without_soh;            /*Byte6:6 48 :55*/
            U8  bms_soc_with_soh;               /*Byte7:7 56 :63*/
        }store;
    }bms_rx_pdo4;

    union
    {
        U8  u8data[8];
        struct
        {
            U32 bms_output_volts:24;              /*Byte0:2 0  :23*/
            U32 bms_max_discharge_power2:24;      /*Byte3:5 24 :47*/
            U16 bms_max_charge_power2;            /*Byte6:7 48 :63*/
        }store;
    }bms_rx_pdo5;

    union
    {
        U8  u8data[8];
        struct
        {
            U8  bms_max_battery_temp;              /*Byte0:0 0  :7*/
            U8  bms_min_battery_temp;              /*Byte1:1 8  :15*/
            U16 bms_max_cell_volts;                /*Byte2:3 16 :31*/
            U16 bms_min_cell_volts;                /*Byte4:5 32 :47*/
        }store;
    }bms_rx_pdo6;

    union
    {
        U8  u8data[8];
        struct
        {
            U8  bms_sdo_reserver_0:4;               /*Byte0:0 0  :4*/
            U8  bms_sdo_scs:3;                      /*Byte0:0 5  :7*/
            U16 bms_sdo_index;                      /*Byte1:2 8  :23*/
            U8  bms_sdo_subindex;                   /*Byte3:3 24 :31*/
            U32 bms_sdo_data;                       /*Byte4:7 32 :63*/
        }store;
    }bms_rx_sdo;
};

struct can_li_ion_tx_signal
{
    union
    {
        U8  u8data[8];
        struct
        {
            U8  bms_sdo_tx_reserver_0:4;               /*Byte0:0 0  :4*/
            U8  bms_sdo_tx_scs:3;                      /*Byte0:0 5  :7*/
            U16 bms_sdo_tx_index;                      /*Byte1:2 8  :23*/
            U8  bms_sdo_tx_subindex;                   /*Byte3:3 24 :31*/
            U32 bms_sdo_tx_data;                       /*Byte4:7 32 :63*/
        }store;
    }bms_tx_sdo;
};


/** Private variables */
struct can_li_ion_private
{
   struct tasklet task_10ms;
   struct tasklet task_1ms;
   struct tasklet task_idle;
   struct can_li_ion_rx_signal rx_signal;
   struct can_li_ion_tx_signal tx_signal;
};


/*===============================================[ private variables ]================================================*/



static struct can_li_ion_private can_li_ion;
/*===============================================[ public variables ]================================================*/




/*===============================================[ private functions ]================================================*/
static void can_ext_rx_process_lilobms_heartbeat(struct canmsg* msg, void *data );
static void can_ext_rx_process_liloBMS_pdo1(struct canmsg* msg, void *data );
static void can_ext_rx_process_liloBMS_pdo2(struct canmsg* msg, void *data );
static void can_ext_rx_process_liloBMS_pdo3(struct canmsg* msg, void *data );
static void can_ext_rx_process_liloBMS_pdo4(struct canmsg* msg, void *data );
static void can_ext_rx_process_liloBMS_pdo5(struct canmsg* msg, void *data );
static void can_ext_rx_process_liloBMS_pdo6(struct canmsg* msg, void *data );
static void can_ext_rx_process_liloBMS_sdo(struct canmsg* msg, void *data );
static void can_ext_li_ion_10ms_service (struct tasklet *task);
static void can_ext_li_ion_1ms_service  (struct tasklet *task);
static void can_ext_li_ion_idle_service (struct tasklet *task);


/*================================================[ inline functions ]================================================*/

/*================================================[ public functions ]================================================*/
RC can_li_ion_initialize( void )
{
   pal_memzero( &can_li_ion,     sizeof can_li_ion );

   tasklet_assign( sched_get_slice_10ms(), &can_li_ion.task_10ms,  can_ext_li_ion_10ms_service, NIL );
   tasklet_assign( sched_get_slice_1ms(),  &can_li_ion.task_1ms,   can_ext_li_ion_1ms_service,  NIL );
   tasklet_assign( sched_get_slice_idle(), &can_li_ion.task_idle,  can_ext_li_ion_idle_service, NIL );

   can_install_rx_callback( can_channel_ext, LILOBMS_HEARTBEAT,    can_ext_rx_process_lilobms_heartbeat,   (void*)can_li_ion.rx_signal.bms_state );
   can_install_rx_callback( can_channel_ext, LILOBMS_PDO1_RX,      can_ext_rx_process_liloBMS_pdo1,        (void*)can_li_ion.rx_signal.bms_rx_pdo1.u8data );
   can_install_rx_callback( can_channel_ext, LILOBMS_PDO2_RX,      can_ext_rx_process_liloBMS_pdo2,        (void*)can_li_ion.rx_signal.bms_rx_pdo2.u8data );
   can_install_rx_callback( can_channel_ext, LILOBMS_PDO3_RX,      can_ext_rx_process_liloBMS_pdo3,        (void*)can_li_ion.rx_signal.bms_rx_pdo3.u8data );
   can_install_rx_callback( can_channel_ext, LILOBMS_PDO4_RX,      can_ext_rx_process_liloBMS_pdo4,        (void*)can_li_ion.rx_signal.bms_rx_pdo4.u8data );
   can_install_rx_callback( can_channel_ext, LILOBMS_PDO5_RX,      can_ext_rx_process_liloBMS_pdo5,        (void*)can_li_ion.rx_signal.bms_rx_pdo5.u8data );
   can_install_rx_callback( can_channel_ext, LILOBMS_PDO6_RX,      can_ext_rx_process_liloBMS_pdo6,        (void*)can_li_ion.rx_signal.bms_rx_pdo6.u8data );
   can_install_rx_callback( can_channel_ext, LILOBMS_SDO_RX,       can_ext_rx_process_liloBMS_sdo,         (void*)can_li_ion.rx_signal.bms_rx_sdo.u8data );
   return RC_SUCCESS;
}


void can_ext_tx_process_liloBMS_sdo(void *data )
{
	can_tx( can_channel_ext, LILOBMS_SDO_TX, data,8);
}

/*===============================================[ private functions ]================================================*/

/** 10ms Service function
*
*   \param task Pointer to calling tasklet.
*
*/
static void can_ext_li_ion_10ms_service (struct tasklet *task)
{
    UNREFERENCED( task );
}

/** 1ms Service function
*
*   \param task Pointer to calling tasklet.
*
*/
static void can_ext_li_ion_1ms_service (struct tasklet *task)
{
    UNREFERENCED( task );
}

/** Idle Service function
*
*   \param task Pointer to calling tasklet.
*
*/
static void can_ext_li_ion_idle_service (struct tasklet *task)
{
    UNREFERENCED( task );
}

/**
*  identifier:    0x70A
*  transmitter: Motor
*  transmission:  cyclically every 40 ms
*
*/
static void can_ext_rx_process_lilobms_heartbeat(struct canmsg* msg, void *data )
{
    U8 buf[2] = {0x01, 0x0D};
    pal_memcpy(data, canmsg_data_ptr(msg), canmsg_get_dlc(msg));

    if(can_li_ion.rx_signal.bms_state == pre_operational)
    {
        can_tx( can_channel_ext, canid_ext_module_control, buf,2);
    }
    else if(can_li_ion.rx_signal.bms_state == stopped)
    {
        buf[0] = enter_pre_operational;
        can_tx( can_channel_ext, canid_ext_module_control, buf,2);
    }
}

/**
*  identifier:    0x481
*  transmitter:   motor lift
*  transmission:  truck display every 1000 ms
*  meaning: CANopen timestamp message for current time and date
*
*/
static void can_ext_rx_process_liloBMS_pdo1(struct canmsg* msg, void *data )
{
    pal_memcpy(data, canmsg_data_ptr(msg), canmsg_get_dlc(msg));
    if(can_li_ion.rx_signal.bms_rx_pdo1.store.bms_battery_current & 0x00800000)
    {
        input_data.bms_battery_current = 0xff000000 | can_li_ion.rx_signal.bms_rx_pdo1.store.bms_battery_current;
    }
    else
    {
        input_data.bms_battery_current = can_li_ion.rx_signal.bms_rx_pdo1.store.bms_battery_current;
    }
    input_data.bms_battery_volts = can_li_ion.rx_signal.bms_rx_pdo1.store.bms_battery_volts;
    input_data.bms_soh           = can_li_ion.rx_signal.bms_rx_pdo1.store.bms_soh;
    input_data.bms_battery_state = can_li_ion.rx_signal.bms_rx_pdo1.store.bms_battery_state;
}
/**
*  identifier:    0x481
*  transmitter:   motor lift
*  transmission:  truck display every 1000 ms
*  meaning: CANopen timestamp message for current time and date
*
*/
static void can_ext_rx_process_liloBMS_pdo2(struct canmsg* msg, void *data )
{
    pal_memcpy(data, canmsg_data_ptr(msg), canmsg_get_dlc(msg));
    
    input_data.bms_max_charge_current2      = can_li_ion.rx_signal.bms_rx_pdo2.store.bms_max_charge_current2;
    input_data.bms_max_discharge_current2   = can_li_ion.rx_signal.bms_rx_pdo2.store.bms_max_discharge_current2;
    input_data.bms_max_charge_current100    = can_li_ion.rx_signal.bms_rx_pdo2.store.bms_max_charge_current100;
    input_data.bms_max_discharge_current100 = can_li_ion.rx_signal.bms_rx_pdo2.store.bms_max_discharge_current100;
}



/**
*  identifier:    0x481
*  transmitter:   motor lift
*  transmission:  truck display every 1000 ms
*  meaning: CANopen timestamp message for current time and date
*
*/
static void can_ext_rx_process_liloBMS_pdo3(struct canmsg* msg, void *data )
{
    pal_memcpy(data, canmsg_data_ptr(msg), canmsg_get_dlc(msg));
    input_data.bms_dtc1 = can_li_ion.rx_signal.bms_rx_pdo3.store.bms_dtc1;
    input_data.bms_dtc2 = can_li_ion.rx_signal.bms_rx_pdo3.store.bms_dtc2;
    input_data.bms_dtc3 = can_li_ion.rx_signal.bms_rx_pdo3.store.bms_dtc3;
    input_data.bms_dtc4 = can_li_ion.rx_signal.bms_rx_pdo3.store.bms_dtc4;
}

/**
*  identifier:    0x481
*  transmitter:   motor traction
*  transmission:  truck display every 1000 ms
*  meaning: CANopen timestamp message for current time and date
*
*/
static void can_ext_rx_process_liloBMS_pdo4(struct canmsg* msg, void *data )
{
	//u16 test_data;
    pal_memcpy(data, canmsg_data_ptr(msg), canmsg_get_dlc(msg));

    input_data.bms_min_discharge_volts = can_li_ion.rx_signal.bms_rx_pdo4.store.bms_min_discharge_volts;
    input_data.bms_max_charge_volts    = can_li_ion.rx_signal.bms_rx_pdo4.store.bms_max_charge_volts;
    input_data.bms_soc_without_soh     = can_li_ion.rx_signal.bms_rx_pdo4.store.bms_soc_without_soh;
    input_data.bms_soc_with_soh        = can_li_ion.rx_signal.bms_rx_pdo4.store.bms_soc_with_soh;
}

/**
*  identifier:    0x481
*  transmitter:   motor traction
*  transmission:  truck display every 1000 ms
*  meaning: CANopen timestamp message for current time and date
*
*/
static void can_ext_rx_process_liloBMS_pdo5(struct canmsg* msg, void *data )
{
    pal_memcpy(data, canmsg_data_ptr(msg), canmsg_get_dlc(msg));
    input_data.bms_output_volts            = can_li_ion.rx_signal.bms_rx_pdo5.store.bms_output_volts;
    input_data.bms_max_discharge_power2    = can_li_ion.rx_signal.bms_rx_pdo5.store.bms_max_discharge_power2;
    input_data.bms_max_charge_power2       = can_li_ion.rx_signal.bms_rx_pdo5.store.bms_max_charge_power2;
}

/**
*  identifier:    0x481
*  transmitter:   motor traction
*  transmission:  truck display every 1000 ms
*  meaning: CANopen timestamp message for current time and date
*
*/
static void can_ext_rx_process_liloBMS_pdo6(struct canmsg* msg, void *data )
{
    pal_memcpy(data, canmsg_data_ptr(msg), canmsg_get_dlc(msg));
    input_data.bms_max_battery_temp = can_li_ion.rx_signal.bms_rx_pdo6.store.bms_max_battery_temp;
    input_data.bms_min_battery_temp = can_li_ion.rx_signal.bms_rx_pdo6.store.bms_min_battery_temp;
    input_data.bms_max_cell_volts   = can_li_ion.rx_signal.bms_rx_pdo6.store.bms_max_cell_volts;
    input_data.bms_min_cell_volts   = can_li_ion.rx_signal.bms_rx_pdo6.store.bms_min_cell_volts;
}

/**
*  identifier:    0x625
*  transmitter:   debug rx pdo0 
*  transmission:  not fixed
*  meaning: test data
*
*/
static void can_ext_rx_process_liloBMS_sdo(struct canmsg* msg, void *data )
{
    pal_memcpy(data, canmsg_data_ptr(msg), canmsg_get_dlc(msg));
    
}



/***********************************************************************************************************************
*                             Copyright 2018 Linde Material Handling. All rights reserved.                             *
***********************************************************************************************************************/
